#!/usr/bin/env python
# coding=UTF-8

import requests
import json
import paho.mqtt.client as mqtt
import threading
import os
import time
import numpy as np
import uuid
import socket
from qcloud_cos import CosConfig
from qcloud_cos import CosS3Client
import os
import logging
import random
import string
from Logger import Logger
import psutil
import serial
import transforms3d as tfs
import math
from Controller import sucker2

secret_id = 'AKIDznLbaJlumNhlY5vhIivthKiqjPq1wb7P'
secret_key = '8KJ3ZrSyKSO8euvhEVXxEG6i5FBsSpzu'
region = 'ap-shanghai'
token = None
scheme = 'https'

config = CosConfig(Region=region, SecretId=secret_id, SecretKey=secret_key, Token=token, Scheme=scheme)
client = CosS3Client(config)

TMS_HOST = "121.5.162.11"
TMS_PORT = "8081"
MQTT_PORT = 1883

SNAPSHOT_URL = "http://192.168.10.100:5000/capture"
# GETIMAGE_URL = "http://192.168.10.92:5000/static/capture_1744176071.jpg"
GETIMAGE_URL = "http://192.168.10.100:5000/"
# FINDSKU_URL = "http://192.168.10.92:5000/findSku?sku_id=7&sku_scale=0.8"
FINDSKU_URL = "http://192.168.10.100:5000/findSku"

ROBOT_HOST = "http://192.168.10.10:9001"
NAV_TARGET_POSE_URL = ROBOT_HOST + "/api/move?location="
NAV_STATUS_URL = ROBOT_HOST + "/api/robot_status"
ROBOT_STOP_URL = ROBOT_HOST + "/api/move/cancel"

TASK_URL = "http://" + TMS_HOST + ":" + TMS_PORT + "/task/getTaskInfo"
CREATE_TASK_URL = "http://" + TMS_HOST + ":" + TMS_PORT + "/task/create"
UUID_URL = "http://" + TMS_HOST + ":" + TMS_PORT + "/robot/getUUID"
UPLOAD_DATA_URL = "http://" + TMS_HOST + ":" + TMS_PORT + "/taskData/create"
# /{taskId}/{taskStatus}/{carId}
UPDATE_TASK_URL = "http://" + TMS_HOST + ":" + TMS_PORT + "/task/updateTaskStatus"

TMS_TASK_COMMAND_TOPIC = "/tms/taskCommand"
#
# # 打开夹爪
# Gripper_release = {"command": "set_gripper_release", "speed": 1000, "force": 1000, "block": True}
# # 闭合夹爪
# Gripper_pick_on = {"command": "set_gripper_pick_on", "speed": 1000, "force": 1000, "block": True}
# 初始点
move_init_angle = {"command": "movej", "joint": [-872, 106673, 4027, -97678, -4880, 111475, -755], "v": 60, "r": 0,
                   "trajectory_connect": 0}
# 拍照点
# {"arm_state":{"err":[8193],"joint":[-2082,38618,-3110,-120092,348,81477,-483],"pose":[-46327,27318,866369,-39,0,-86]},"state":"current_arm_state"}

photo_angle = {"command": "movej", "joint": [-2082,38618,-3110,-120092,348,81477,-483], "v": 60, "r": 0,
                    "trajectory_connect": 0}
# 放置过渡点1
put_guodudian1 = {"command": "movej", "joint": [-98995,38688,-5041,-120031,-572,80886,99255], "v": 60, "r": 0,
               "trajectory_connect": 0}

# 放置过渡点2
put_guodudian2 = {"command": "movej", "joint": [-91966,38478,-5177,-120341,-3375,-48121,88868], "v": 20, "r": 0,
                  "trajectory_connect": 0}
# 放置点
put_angle= {"command": "movej", "joint": [-91793,-11222,-7896,-120058,1783,-50888,82514], "v": 20, "r": 0,
                  "trajectory_connect": 0}
# # 放置过渡点3
# put_guodudian3 = {"command": "movej", "joint": [-102730, -73880, -16460, -102051, 77601, -13161, 3324], "v": 20, "r": 0,
#                   "trajectory_connect": 0}
# # 放置点
# put_angle = {"command": "movej", "joint": [-134390, -103462, 1892, -82803, 49316, -82298, -2982], "v": 20, "r": 0,
#              "trajectory_connect": 0}
# 升降下降
lift_down = {"command": "set_lift_height", "height": 225, "speed": 100}
#
# # 平放放置的过渡点
# put_guodudian1 = {"command": "movej", "joint": [-106582, -64528, -113058, 16212, -2083, 62038], "v": 60, "r": 0,"trajectory_connect": 0}

# # 平放的放置点
# put_final = {"command": "movej", "joint": [-139145, -95548, -73100, 39862, -97329, 2298], "v": 60, "r": 0,
#              "trajectory_connect": 0}

# 升降默认高度
lift_up_default = {"command": "set_lift_height", "height": 500, "speed": 100}

STORE_MQTT_BROKER = "192.168.1.8"
STORE_MQTT_CLIENT_ID = "robot_zhaji_xl04"
MQTT_TOPIC_SEND = "sensing.robot.controller.tenant-5089.event"
MQTT_TOPIC_RECV = "sensing.robot.controller.tenant-5089.message"

grip_port = 'COM3'
grip_baudrate = 115200

HOST_ARM = '192.168.10.18'
PORT_ARM = 8080

ARM_HOST_HTTP = "http://192.168.10.18:8090"

# 拍照姿态
cap_pose = [676, 108155, 314449, -3129, -1379, 1583]
home_pose = [-238074, 10093, 484621, -2989, -1199, 2942]

# 过渡姿态=固定X，目标姿态的Y-130mm，目标姿态的Z+40mm
pose_guodu = [7262, -130000, 40000, 2157, -1534, 2542]
# 目标姿态上方=目标姿态的Z+40mm
pose_target = [0, 0, 40000, 2157, -1534, 2542]
# 抓取后过渡点1
pick_guodu_1 = [-215916, 295073, 317045, -3054, -1529, 3105]
# 抓取后过渡点2
pick_guodu_2 = [-201486, 290815, 502616, 46, -1148, -22]

# 穴位1
hole_1_end = [-185620, -131094, 189328, 2540, -1559, -940]
# 穴位1
hole_1_up = [-185620, -131094, 270328, 2540, -1559, -940]
# 穴位1
hole_1_up2 = [-185620, -131094, 290328, 2540, -1559, -940]

hand2cam_tf_matrix = [
    [0.0, -1.0, 0.0, 182.5],
    [1.0, 0.0, 0.0, -34.5],
    [0.0, 0.0, 1.0, -152.0],
    [0.0, 0.0, 0.0, 1.0]
]


class Task:
    def __init__(self, name="robot"):
        self.name = "gxy_robot"
        self.car_id = "unbind"
        self.robotCode = ""
        self.task_status = "1"
        self.task_id = None
        self.job_type = None
        self.time_out = 300
        self.wait_power = 80
        self.low_power = 50
        self.current_point = 0
        self.log = Logger('all.log', level='debug')
        self.continue_loop = False
        self.confirm = False
        self.robot_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.robot_socket.connect((HOST_ARM, PORT_ARM))
        self.suck = sucker2.XiPan()
        self.R_cam2gripper = np.array([
            [0, 1, 0],
            [-1, 0, 0],
            [0, 0, 1]
        ])
        self.T_cam2gripper = np.array([0.092, 0.0, 0.121])
        self.token_v = "1581093759N9gG74hsi8uDRx7m82fv0VDRchV17F3k"

    def mqtt_init(self, debug=False):
        self.client = mqtt.Client()
        self.client.on_connect = self.on_connect
        self.client.on_message = self.on_message
        self.client.connect("" + TMS_HOST + "", MQTT_PORT, 60)
        self.client.loop_start()  # 在新线程中启动网络循环

        if debug:
            self.store_username = "admin"
            self.store_password = "1q2w3e4r"
            self.store_client = mqtt.Client(STORE_MQTT_CLIENT_ID)
            self.store_client.on_message = self.on_message_store
            self.store_client.on_connect = self.on_connect_store
            self.store_client.username_pw_set(self.store_username, self.store_password)
            self.store_client.connect(STORE_MQTT_BROKER, 1883)
            self.store_client.loop_start()

        # thread = threading.Thread(target=self.client.loop_forever)
        # thread.start()

    # MQTT回调函数，处理接收到的消息
    def on_message_store(self, client, userdata, msg):
        payload = msg.payload.decode("utf-8")
        data = json.loads(payload)
        print(data)
        self.continue_loop = True
        # print(type(data['flag']))
        # if data['flag']:
        #     print(f"Received flag: {data}, continuing loop.")
        #     # 设置一个全局变量或者通过其他方式通知主循环继续
        #     global continue_loop
        #     continue_loop = True

    # MQTT客户端连接建立后的回调
    def on_connect_store(self, client, userdata, flags, rc):
        print(f"Connected with result code {rc}")
        # 订阅接收主题
        client.subscribe(MQTT_TOPIC_RECV)

    def map_init(self):
        with open(MAP_PATH, 'rb') as file:
            local_data = file.read()
        response = requests.put(MAP_URL, data=local_data)
        if response.status_code == 200:
            server_data = response.text
            self.log.logger.info(server_data)
        else:
            self.log.logger.info(f'请求失败，状态码：{response.status_code}')

    def on_connect(self, client, userdata, flags, rc):
        self.log.logger.info("Connected with result code" + str(rc))
        client.subscribe(TMS_TASK_COMMAND_TOPIC)
        client.subscribe("/robot/" + self.robotCode + "/confirm")
        print("===============sub topic =================")
        print("/robot/" + self.robotCode + "/confirm")

    def on_message(self, client, userdata, msg):
        self.log.logger.info(msg.topic)
        topic_str = msg.topic
        if topic_str == TMS_TASK_COMMAND_TOPIC:
            self.log.logger.info(msg.payload)
            data = msg.payload.decode()
            item = json.loads(data)
            taskId = item['taskId']
            print("current taskid = [" + str(self.task_id) + "]")
            if str(self.task_id) == taskId:
                command = item['command']
                self.log.logger.info(type(command))
                if 0 == int(command):
                    # 停止导航
                    self.robot_stop()
        elif topic_str == "/robot/" + self.robotCode + "/confirm":
            self.log.logger.info(msg.payload)
            data = msg.payload.decode()
            item = json.loads(data)
            if item['result'] == True:
                self.confirm = True

    def generate_random_string(self, length):
        characters = string.ascii_letters + string.digits
        return ''.join(random.choice(characters) for _ in range(length))

    def euler_to_quaternion(self, yaw, pitch, roll):
        qx = np.sin(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) - np.cos(roll / 2) * np.sin(pitch / 2) * np.sin(
            yaw / 2)
        qy = np.cos(roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2) + np.sin(roll / 2) * np.cos(pitch / 2) * np.sin(
            yaw / 2)
        qz = np.cos(roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2) - np.sin(roll / 2) * np.sin(pitch / 2) * np.cos(
            yaw / 2)
        qw = np.cos(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) + np.sin(roll / 2) * np.sin(pitch / 2) * np.sin(
            yaw / 2)
        return [qx, qy, qz, qw]

    def getCapture(self, cap_url, task_id, site_name):
        random_string = self.generate_random_string(5)
        filename = str(task_id) + '/image_' + str(site_name) + '_' + str(random_string) + '.jpg'
        cap_name = 'https://robot-1312773182.cos.ap-shanghai.myqcloud.com/' + filename
        response = requests.get(cap_url)
        content = response.content
        if self.job_type == '4' or self.job_type == '5':
            self.upload_data(content, filename)
        self.log.logger.info(cap_url + "抓图成功")
        return cap_name

    def robot_stop(self):
        batteryPercentage = 0
        response = requests.get(ROBOT_STOP_URL)
        if response.status_code == 200:
            print("终止导航成功...")
            self.task_status = "0"

    def get_power(self):
        batteryPercentage = 0
        response = requests.get(ROBOT_POWER_URL)
        if response.status_code == 200:
            data = json.loads(response.content)
            batteryPercentage = data['batteryPercentage']
            isCharging = data['isCharging']
            if isCharging:
                self.current_point = 3

        return batteryPercentage

    def go_charge(self):
        param = {}
        param['action_name'] = "slamtec.agent.actions.GoHomeAction"
        gohome_options = {}
        gohome_options['flags'] = 'dock'
        gohome_options['back_to_landing'] = True
        gohome_options['charging_retry_count'] = 3
        move_options = {}
        move_options['mode'] = 0
        gohome_options['move_options'] = move_options
        options = {}
        options['gohome_options'] = gohome_options
        param['options'] = options
        self.log.logger.info(param)
        headers = {
            'Content-type': 'application/json',
            'Accept': 'application/json'
        }
        response = requests.post(url=NAV_TARGET_URL, json=param, headers=headers)
        self.log.logger.info(response.status_code)
        if response.status_code == 200:
            data = json.loads(response.content)
            self.log.logger.info(data)

    def send_goals_python(self, pos, count, task, site_goods_dict, site_name):
        nav_url = NAV_TARGET_POSE_URL + str(pos[0]) + "," + str(pos[1]) + "," + str(pos[2]) + ""
        self.log.logger.info("触发底盘导航=" + str(nav_url))
        response = requests.get(url=nav_url)
        if response.status_code == 200:
            data = json.loads(response.content)
            print(data)
        self.log.logger.info(response.status_code)

        if response.status_code == 200:
            data = json.loads(response.content)
            self.log.logger.info(data)
            task_id = data['results']['task_id']
            str_log = "Send NO. %s Goal !!!" % str(pos)
            self.log.logger.info(str_log)
            while True:
                response = requests.get(NAV_STATUS_URL)
                data = json.loads(response.content)
                if response.status_code == 200:
                    reach_data = json.loads(response.content)
                    reach_task_id = reach_data['results']['task_id']
                    reach_move_status = reach_data['results']['move_status']
                    if reach_task_id == task_id and reach_move_status == "succeeded":
                        str_log = "Reach NO. %s Goal !!!" % str(pos)
                        self.log.logger.info(str_log)
                        break
                    else:
                        time.sleep(1)

            # 到达位置后，
            content = None
            time.sleep(1)

            # 上集成部分 ============begin===================
            cos_img_list = []
            site_type = task['site_type']
            if site_type == "2":
                return "Mission Finished. because site_type is 2"

            # 打开夹爪
            self.grip("open")
            self.log.logger.info("该点位要抓的商品集合=" + str(site_goods_dict))
            task_type_str = task['task_type']
            # task_type_str = "0:1"
            for pair_str in task_type_str.split(','):
                # 对每个键值对进行分割
                key_str, quantity_str = pair_str.split(':')
                # 转换键为整数并查找对应的名称
                sku_id = int(key_str)
                for i in range(int(quantity_str)):
                    # 升降控制
                    if key_str in site_goods_dict:
                        height = int(site_goods_dict[key_str])
                        print("升降高度" + str(height))
                        lift_up = {"command": "set_lift_height", "height": height, "speed": 100}
                        self.send_arm_command(lift_up)
                        self.send_arm_command(move_init_angle)
                        # self.send_arm_command(photo_guodudian1)
                        self.send_arm_command(photo_angle)
                        time.sleep(0.5)
                        try:
                            # 找SKU
                            for i in range(5):
                                result = self.find_sku(sku_id, "0.5")
                                if not result:
                                    self.log.logger.error(f"未找到SKU:{sku_id}")
                                if result:
                                    image_url = GETIMAGE_URL + result['image_url']
                                    self.log.logger.info(f"获取到了图片URL{image_url}")
                                    mindepth_box = result['mindepth_box']
                                    target_xyz = result['target_xyz']
                                    # 图片上传到COS
                                    cos_img_path = self.getCapture(image_url, task_id, site_name)
                                    cos_img_list.append(cos_img_path)
                                    # 机械臂抓取
                                    self.log.logger.info("相机坐标系下的目标坐标=" + str(target_xyz))

                                    # 将相机坐标系XYZ，转换为机械臂arm_base的XYZ
                                    pose_t, pose_R = self.t_camxyz_armxyz(target_xyz)

                                    # 升降最高处940，最低处0，实际差距836mm
                                    offset_height = pose_t[0] / 1000 * 1.65
                                    # # 上升到抓取时高度
                                    lift_high_up = {"command": "set_lift_height", "height": height + offset_height,
                                                    "speed": 100}
                                    self.send_arm_command(lift_high_up)
                                    # 气泵打开
                                    self.suck.open_xipan()
                                    # 抓取点前方
                                    # {"command": "movej_p","pose": [794869, 152503, 771, 1574, -1492, 1635],"v": 20, "r": 0, "trajectory_connect": 0}
                                    move_pick = {"command": "movej_p",
                                                 "pose": [-46327, pose_t[1] + 6000, pose_t[2]+500, 44, 11, -74],
                                                 "v": 20, "r": 0, "trajectory_connect": 0}
                                    self.send_arm_command(move_pick)

                                    # 抓取点
                                    move_pick = {"command": "movej_p",
                                                 "pose": [-46327, pose_t[1] + 6000, pose_t[2] + 6000, 44, 11, -74],
                                                 "v": 10, "r": 0, "trajectory_connect": 0}
                                    self.send_arm_command(move_pick)
                                    # self.grip("close")
                                    time.sleep(2)

                                    # 抓取点上方
                                    lift_high_up = {"command": "set_lift_height",
                                                    "height": height + offset_height + 35, "speed": 100}
                                    self.send_arm_command(lift_high_up)

                                    # 抓取点前方
                                    move_pick = {"command": "movej_p",
                                                 "pose": [-46327, pose_t[1] + 6000, pose_t[2]+500, 44, 11, -74],
                                                 "v": 20, "r": 0, "trajectory_connect": 0}
                                    self.send_arm_command(move_pick)
                                    self.send_arm_command(photo_angle)
                                    lift_high_up = {"command": "set_lift_height",
                                                    "height": height + offset_height + 350, "speed": 100}
                                    self.send_arm_command(put_guodudian1)
                                    if height < 200:
                                        self.send_arm_command(lift_high_up)
                                    self.send_arm_command(put_guodudian1)
                                    self.send_arm_command(put_guodudian2)
                                    # self.send_arm_command(put_guodudian3)
                                    self.send_arm_command(put_angle)
                                    self.send_arm_command(lift_down)
                                    # self.grip("open")
                                    self.suck.close_xipan()
                                    time.sleep(2)
                                    self.send_arm_command(lift_up_default)
                                    # self.send_arm_command(put_guodudian3)
                                    self.send_arm_command(put_guodudian2)
                                    self.send_arm_command(put_guodudian1)
                                    self.send_arm_command(photo_angle)
                                    self.send_arm_command(move_init_angle)
                                    break
                                time.sleep(0.2)
                        except Exception as e:
                            self.log.logger.info("==============回初始位置===========:", e)
                        # 机械臂回初始位置
                        # self.send_arm_command(photo_angle)
                        self.send_arm_command(move_init_angle)

            # 将图片上传至TMS
            param = {}
            param['taskId'] = task['task_id']
            param['deviceName'] = task['device_name']
            param['siteName'] = task['site_name']
            param['siteCoordinate'] = task['site_coordinate']
            param['dataInfo'] = "rfid_str"
            param['carId'] = self.car_id
            for i in range(len(cos_img_list)):
                param['camera' + str(i + 1)] = cos_img_list[i]
            self.log.logger.info(param)
            headers = {
                'Content-type': 'application/json',
                'Accept': 'application/json'
            }
            response = requests.post(url=UPLOAD_DATA_URL, json=param, headers=headers)
            self.log.logger.info(response.status_code)
            if response.status_code == 200:
                data = json.loads(response.content)
                self.log.logger.info(data)

            # site_type = task['site_type']
            # if site_type == "2":
            #     return "Mission Finished. because site_type is 2"

            # # task_type_str = task['task_type']
            # task_type_str = "0:1"
            # for pair_str in task_type_str.split(','):
            #     # 对每个键值对进行分割
            #     key_str, quantity_str = pair_str.split(':')
            #     # 转换键为整数并查找对应的名称
            #     yinliao_id = int(key_str)
            #     for i in range(int(quantity_str)):
            #         # 机械臂去拍照位置
            #         self.arm_go_capture()
            #         # 请求相机拍照，根据要拍的物品和得分，返回物体的相机坐标系下XYZ

            #         for i in range(10):
            #             result = self.trigger_capture(yinliao_id,0.5)
            #             if result and result['code']=="200":
            #                 cam_xyz = result['data']
            #                 # 将相机坐标系XYZ，转换为机械臂arm_base的XYZ
            #                 pose_t, pose_R = self.t_camxyz_armxyz(cam_xyz)
            #                 # 机械臂抓取动作组
            #                 self.arm_pick_group(pose_t, pose_R)
            #                 break
            # 上集成部分 ============end===================

        return "Mission Finished."

    def send_arm_command(self, command, max_retries=3):
        """
        发送json指令给机械臂动作，并根据device值处理响应，带重试机制
        :param command: json格式的机械臂命令
        :param max_retries: 最大重试次数
        :return: None
        """
        buffer = ""  # 用于累积接收到的数据
        first_message = None  # 存储第一条消息
        target_message = None  # 存储匹配目标device的消息
        retries = 0  # 重试计数

        if not self.robot_socket:
            print("未连接到机械臂")
            return None

        # 将字典转换为JSON字符串
        json_str = json.dumps(command)
        self.robot_socket.sendall((json_str + '\n').encode('utf-8'))
        print(f"已发送机械臂指令: {json_str}")

        while retries < max_retries:
            try:
                while True:
                    try:
                        response = self.robot_socket.recv(1024).decode('utf-8')
                        if not response:
                            print("服务端断开连接")
                            break
                        buffer += response

                        while '\n' in buffer:
                            message, buffer = buffer.split('\n', 1)
                            print("收到的消息")
                            print(message)
                            try:
                                parsed_message = json.loads(message)
                                print(f"Received message: {parsed_message}")

                                # 如果是第一条消息，存储用于state检查
                                if first_message is None:
                                    first_message = parsed_message

                                # 检查device值
                                device_value = parsed_message.get('device')
                                if device_value == 3:
                                    print(f"检测到device=3，跳出循环")
                                    return  # 直接返回

                                # 如果device为0或1，且还未找到目标消息
                                if device_value in (0, 1) and target_message is None:
                                    target_message = parsed_message
                                    state_ok = (first_message.get('state', False) == True or
                                                first_message.get('receive_state', False) == True)
                                    trajectory_state_ok = target_message.get('trajectory_state', False) == True
                                    print(f"State (from first message) OK: {state_ok}, "
                                          f"Trajectory state (from target message) OK: {trajectory_state_ok}")
                                    if state_ok and trajectory_state_ok:
                                        print(f"指令执行完成，device={device_value}，状态验证通过")
                                        return
                                    else:
                                        print("指令执行未完成或状态异常:")
                                        print(f"First message: {first_message}")
                                        print(f"Target message: {target_message}")
                                        raise RuntimeError("机械臂指令执行失败，状态未达到预期")

                            except json.JSONDecodeError:
                                buffer = message + '\n' + buffer
                                break
                    except socket.timeout:
                        print(f"接收响应超时（第 {retries + 1} 次尝试），可能是机械臂未正确响应")
                        if first_message and target_message:
                            state_ok = (first_message.get('state', False) == True or
                                        first_message.get('receive_state', False) == True)
                            trajectory_state_ok = target_message.get('trajectory_state', False) == True
                            if state_ok and trajectory_state_ok:
                                print(f"指令执行完成，超时前已收到有效消息，状态验证通过")
                                return
                        retries += 1
                        if retries < max_retries:
                            print(f"重试 {retries + 1}/{max_retries}")
                            buffer = ""  # 清空缓冲区，重试
                            self.robot_socket.sendall((json_str + '\n').encode('utf-8'))  # 重新发送指令
                            sleep(1)  # 短暂等待后重试
                        break

            except socket.timeout:
                continue  # 外层循环处理重试

        raise RuntimeError(f"机械臂响应超时，经过 {max_retries} 次重试仍未成功")

    def get_arm_state(self, command, max_retries=3):
        """
        发送json指令给机械臂获取机器人当前状态
        :param command: json格式的机械臂命令
        :param max_retries: 最大重试次数
        :return: joint,pose
        """
        buffer = ""  # 用于累积接收到的数据
        first_message = None  # 存储第一条消息
        target_message = None  # 存储匹配目标device的消息
        retries = 0  # 重试计数

        if not self.robot_socket:
            print("未连接到机械臂")
            return None

        # 将字典转换为JSON字符串
        json_str = json.dumps(command)
        self.robot_socket.sendall((json_str + '\n').encode('utf-8'))
        print(f"已发送机械臂指令: {json_str}")
        joint = []
        pose = []
        while retries < max_retries:
            try:
                while True:
                    try:
                        response = self.robot_socket.recv(1024).decode('utf-8')
                        if not response:
                            print("服务端断开连接")
                            break
                        buffer += response

                        while '\n' in buffer:
                            message, buffer = buffer.split('\n', 1)
                            print("收到的消息")
                            print(message)
                            try:
                                parsed_message = json.loads(message)
                                print(f"Received message: {parsed_message}")

                                joint = parsed_message['arm_state']['joint']
                                pose = parsed_message['arm_state']['pose']
                                print("机械臂当前joint=" + str(joint) + ",pose=" + str(pose) + "")
                                return joint, pose

                            except json.JSONDecodeError:
                                buffer = message + '\n' + buffer
                                break
                    except socket.timeout:
                        print(f"接收响应超时（第 {retries + 1} 次尝试），可能是机械臂未正确响应")
                        retries += 1
                        if retries < max_retries:
                            print(f"重试 {retries + 1}/{max_retries}")
                            buffer = ""  # 清空缓冲区，重试
                            self.robot_socket.sendall((json_str + '\n').encode('utf-8'))  # 重新发送指令
                            sleep(1)  # 短暂等待后重试
                        break

            except socket.timeout:
                continue  # 外层循环处理重试
        return joint, pose

    def set_tool_frame(self, command, max_retries=3):
        """
        发送json指令给机械臂获设置工具坐标系
        :param command: json格式的机械臂命令
        :param max_retries: 最大重试次数
        :return: joint,pose
        """
        buffer = ""  # 用于累积接收到的数据
        first_message = None  # 存储第一条消息
        target_message = None  # 存储匹配目标device的消息
        retries = 0  # 重试计数

        if not self.robot_socket:
            print("未连接到机械臂")
            return None

        # 将字典转换为JSON字符串
        json_str = json.dumps(command)
        self.robot_socket.sendall((json_str + '\n').encode('utf-8'))
        print(f"已发送机械臂指令: {json_str}")
        while retries < max_retries:
            try:
                while True:
                    try:
                        response = self.robot_socket.recv(1024).decode('utf-8')
                        if not response:
                            print("服务端断开连接")
                            break
                        buffer += response

                        while '\n' in buffer:
                            message, buffer = buffer.split('\n', 1)
                            print("收到的消息")
                            print(message)
                            try:
                                parsed_message = json.loads(message)
                                print(f"Received message: {parsed_message}")
                                flag = parsed_message['change_tool_frame']
                                return flag

                            except json.JSONDecodeError:
                                buffer = message + '\n' + buffer
                                break
                    except socket.timeout:
                        print(f"接收响应超时（第 {retries + 1} 次尝试），可能是机械臂未正确响应")
                        retries += 1
                        if retries < max_retries:
                            print(f"重试 {retries + 1}/{max_retries}")
                            buffer = ""  # 清空缓冲区，重试
                            self.robot_socket.sendall((json_str + '\n').encode('utf-8'))  # 重新发送指令
                            sleep(1)  # 短暂等待后重试
                        break

            except socket.timeout:
                continue  # 外层循环处理重试
        return joint, pose

    def arm_pick_group(self, pose_t, pose_R):

        target_offset_x = -15000
        target_offset_y = -130000
        # 机械臂去目标点前方
        target_front = pose_guodu
        target_front[0] = int(pose_t[0] + target_offset_x)
        target_front[1] = int(pose_t[1] + target_offset_y)
        target_front[2] = int(pose_t[2])
        print("机械臂去目标点前方---抓取前")
        self.arm_go(target_front, "movej_p")

        # 打开夹爪
        self.grip(1000)

        # 机械臂去目标点
        go_pose = pose_target
        go_pose[0] = int(pose_t[0] + target_offset_x)
        go_pose[1] = int(pose_t[1] + 10000)
        go_pose[2] = int(pose_t[2] - 8000)
        print("机械臂去目标点")
        self.arm_go(go_pose, "movej_p")

        # 闭合夹爪
        self.grip(10)

        # 机械臂去目标点上方
        target_head = pose_guodu
        target_head[0] = int(pose_t[0] + target_offset_x)
        target_head[1] = int(pose_t[1] + 10000)
        target_head[2] = int(pose_t[2] + 30000)
        print("机械臂去目标点上方---抓取后")
        self.arm_go(target_head, "movej_p")

        # 机械臂去目标点上方的前方
        target_front = pose_guodu
        target_front[0] = int(pose_t[0] + target_offset_x)
        target_front[1] = int(pose_t[1] + target_offset_y)
        target_front[2] = int(pose_t[2] + 30000)
        print("机械臂去目标点上方的前方---抓取后")
        self.arm_go(target_front, "movej_p")

        # 机械臂去目标点上方的前方的上方
        target_front = pose_guodu
        target_front[0] = int(pose_t[0] + target_offset_x)
        target_front[1] = int(pose_t[1] + target_offset_y)
        target_front[2] = int(pose_t[2] + 120000)
        print("机械臂去目标点上方的前方的上方---抓取后")
        self.arm_go(target_front, "movej_p")

        print("机械臂去抓取后过渡点1")
        self.arm_go(pick_guodu_1, "movej_p")

        print("机械臂去抓取后过渡点2")
        self.arm_go(pick_guodu_2, "movej_p")

        print("机械臂去抓取后过渡点1")
        self.arm_go(pick_guodu_1, "movej_p")

        print("机械臂去抓取后穴位1上")
        self.arm_go(hole_1_up, "movej_p")

        print("机械臂去抓取后穴位1下")
        self.arm_go(hole_1_end, "movej_p")

        # 打开夹爪
        self.grip(1000)

        print("机械臂去抓取后穴位1上")
        self.arm_go(hole_1_up, "movej_p")

        print("机械臂去抓取后穴位1上")
        self.arm_go(hole_1_up2, "movej_p")

        # 机械臂去home位置
        self.arm_go_home()

    def t_camxyz_armxyz(self, cam_xyz):

        # 手眼矩阵 x 相机坐标系下物理坐标 = 物体到工具坐标系矩阵
        pose_end = np.matmul(hand2cam_tf_matrix, self.xyz_euler_to_mat(cam_xyz, (0, 0, 0)))  # 转换的末端相对坐标
        print("目标到工具坐标系位置=如下==")
        print(pose_end)

        # 获取机械臂TCP矩阵
        get_arm_state_command = {"command": "get_current_arm_state"}
        joint, pose = self.get_arm_state(get_arm_state_command)
        print("调用函数返回，机械臂当前joint=" + str(joint) + ",pose=" + str(pose) + "")

        current_pose = pose
        np_current_pose = np.array(current_pose) / 1000
        current_pose = np_current_pose.tolist()
        position = [current_pose[0], current_pose[1], current_pose[2]]
        er = (current_pose[3], current_pose[4], current_pose[5])
        endpoint = self.xyz_euler_to_mat(position, er, False)
        print("机械手当前位置=如下==")
        print(endpoint)

        # 末端TCP矩阵 x 物体到工具坐标矩阵 = 物体到arm基坐标矩阵
        world_pose = np.matmul(endpoint, pose_end)  # 转换到机械臂世界坐标
        pose_t, pose_R = self.mat_to_xyz_euler(world_pose)
        print("目标到基坐标位置=如下==")
        print(pose_t)
        pose_t[0] = int(pose_t[0] * 1000)
        pose_t[1] = int((pose_t[1]) * 1000)
        pose_t[2] = int((pose_t[2]) * 1000)
        print(pose_t)
        print("目标到基坐标旋转=如下==")
        print(pose_R)
        return pose_t, pose_R

    def xyz_euler_to_mat(self, xyz, euler, degrees=True):
        if degrees:
            mat = tfs.euler.euler2mat(math.radians(euler[0]), math.radians(euler[1]), math.radians(euler[2]))
        else:
            mat = tfs.euler.euler2mat(euler[0], euler[1], euler[2])
        mat = tfs.affines.compose(np.squeeze(np.asarray(xyz)), mat, [1, 1, 1])
        return mat

    def mat_to_xyz_euler(self, mat, degrees=True):
        t, r, _, _ = tfs.affines.decompose(mat)
        if degrees:
            euler = np.degrees(tfs.euler.mat2euler(r))
        else:
            euler = tfs.euler.mat2euler(r)
        return t, euler

    def find_sku(self, sku_id, sku_scale):
        # 请求数据
        url = FINDSKU_URL + "?sku_id=" + str(sku_id) + "&sku_scale=" + str(sku_scale)
        response = requests.get(url)
        if response.status_code == 200:
            result = response.json()
            print(result)
            print("触发相机拍照成功")
            return result

    def get_arm_pose(self):
        param = {}
        param['command'] = "get_current_arm_state"
        json_str = json.dumps(param) + "\r\n"
        print(json_str)
        data = bytes(json_str, encoding='utf-8')
        self.s.sendall(data)
        msg = self.s.recv(1024)
        res = json.loads(msg.decode('utf-8'))
        pose = res['arm_state']['pose']
        time.sleep(0.5)
        print("==============获取机械臂当前位置成功=================")
        return pose

    def arm_go_home(self):
        param = {}
        param['command'] = "movej_p"
        param['pose'] = home_pose
        param['v'] = 20
        param['r'] = 0
        json_str = json.dumps(param) + "\r\n"
        print(json_str)
        data = bytes(json_str, encoding='utf-8')
        self.s.sendall(data)
        msg = self.s.recv(1024)
        print(msg.decode('utf-8'))
        print("机械臂去Home位置成功")

    def arm_go_capture(self):
        self.arm_go(cap_pose, "movej_p")
        print("机械臂去拍照位置成功")

    def arm_go(self, pose, command):
        param = {}
        param['command'] = command
        param['pose'] = pose
        param['v'] = 20
        param['r'] = 0
        json_str = json.dumps(param) + "\r\n"
        print(json_str)
        data = bytes(json_str, encoding='utf-8')
        self.s.sendall(data)
        msg = self.s.recv(1024)
        print(msg.decode('utf-8'))
        time.sleep(0.5)

    def getTask(self):
        get_task_url = TASK_URL + '/' + str(self.car_id) + '/1,2,3'
        self.log.logger.info(get_task_url)
        timer = 0
        while True:
            response = requests.get(get_task_url)
            if response.status_code == 200:
                data = json.loads(response.content)
                if data['code'] == 200:
                    tasks = data['data']
                    if len(tasks) > 0:
                        timer = 0
                        # 更新任务状态到导航中
                        task_id = tasks[0]['task_id']
                        self.task_id = task_id
                        job_type = tasks[0]['job_type']
                        self.job_type = job_type
                        task_status_url = UPDATE_TASK_URL + "/" + str(task_id) + "/3/" + str(self.car_id)
                        self.log.logger.info(task_status_url)
                        response = requests.get(url=task_status_url)
                        self.log.logger.info(response.status_code)
                        if response.status_code == 200:
                            data = json.loads(response.content)
                            self.log.logger.info(data)

                        # 任务中需要取的商品和数量
                        task_goods_list = []
                        task_type_str = tasks[0]['task_type']
                        for pair_str in task_type_str.split(','):
                            # 对每个键值对进行分割
                            key_str, quantity_str = pair_str.split(':')
                            # 转换键为整数并查找对应的名称
                            task_goods_id = int(key_str)
                            task_goods_list.append(task_goods_id)
                            task_goods_count = int(quantity_str)

                        count = 1
                        cancel_status = 0
                        for task in tasks:
                            if self.task_status == "0":
                                task_status_url = UPDATE_TASK_URL + "/" + str(task_id) + "/7/" + str(self.car_id)
                                self.log.logger.info("quxiao renwu = " + str(task_status_url))
                                self.task_status = "1"
                                response = requests.get(url=task_status_url)
                                self.log.logger.info(response.status_code)
                                if response.status_code == 200:
                                    self.log.logger.info(str(self.task_id) + "任务取消完成")
                                    cancel_status = 1
                                    break

                            site_name = task['site_name']
                            site_type = task['site_type']
                            site_describe = task['site_describe']
                            # 作业点判断该点位是否有任务中需要的商品，非作业点不需要判断
                            if site_describe is None and site_type == "1":
                                self.log.logger.info(str(task['site_describe']) + "无商品")
                                continue
                            if site_type == "1":
                                site_goods = site_describe.split(",")
                                self.log.logger.info("该点位的商品集合site_goods=" + str(site_goods))
                                site_goods_dict = {}
                                for _ in site_goods:
                                    good_id, lift_value = _.split(':')
                                    if int(good_id) in task_goods_list:
                                        site_goods_dict[good_id] = lift_value
                                if len(site_goods_dict) == 0:
                                    # 该站点没有要取的商品
                                    self.log.logger.info(str(site_name) + "该站点没有该任务需要的商品")
                                    continue

                            site_coordinate = task['site_coordinate']

                            if site_coordinate is None:
                                self.log.logger.info(str(task['device_name']) + "无点位")
                                continue

                            self.log.logger.info(type(site_coordinate))
                            pos = site_coordinate.split(",")
                            self.log.logger.info(type(pos))
                            self.log.logger.info(pos)
                            site_is_trigger = task['site_is_trigger']
                            # mqtt 发布任务类型为API的消息
                            if self.job_type == '5':
                                self.client.publish("/robot/" + str(self.robotCode) + "/taskinfo", json.dumps(
                                    {"taskType": "API", "msg": "正在前往" + str(site_name), "taskStatus": "2",
                                     "istrigger": str(site_is_trigger)}))

                            # 判断点位类型是否需要触发外部设备
                            if site_is_trigger == '1':
                                command = {}
                                command['Subkey'] = "33682db75cff447db210e186561423a4"
                                command['ActionName'] = "RobotEnter"
                                print("send command = " + str(command))
                                self.store_client.publish(MQTT_TOPIC_SEND, str(command), 1)
                                time.sleep(2)
                                # while not self.continue_loop:
                                #     time.sleep(0.2)
                            elif site_is_trigger == '2':
                                self.client.publish("/robot/" + str(self.robotCode) + "/taskinfo", json.dumps(
                                    {"taskType": "API", "msg": "准备前往" + str(site_name), "taskStatus": "2",
                                     "istrigger": str(site_is_trigger)}))
                                while True:
                                    if self.confirm == True:
                                        self.client.publish("/robot/" + str(self.robotCode) + "/taskinfo", json.dumps(
                                            {"taskType": "API", "msg": "正在前往" + str(site_name), "taskStatus": "2",
                                             "istrigger": "0"}))
                                        print("人为确认，释放阻塞")
                                        self.confirm = False
                                        break
                                    else:
                                        time.sleep(1)

                            self.send_goals_python(pos, count, task, site_goods_dict, site_name)
                            count = count + 1
                        if cancel_status == 0:
                            task_status_url = UPDATE_TASK_URL + "/" + str(task_id) + "/5/" + str(self.car_id)
                            self.log.logger.info(task_status_url)
                            response = requests.get(url=task_status_url)
                            self.log.logger.info(response.status_code)
                            if response.status_code == 200:
                                self.log.logger.info(str(self.task_id) + "任务正常完成")
                                self.log.logger.info("job_type=" + str(self.job_type))
                                # mqtt 发布任务类型为API的消息
                                self.client.publish("/robot/" + str(self.robotCode) + "/taskinfo", json.dumps(
                                    {"taskType": "API", "msg": "正在前往" + str(site_name), "taskStatus": "5"}))

                                if self.job_type == '4':
                                    self.go_charge()
                        self.task_status = "1"
                    else:
                        self.log.logger.info("没有任务，休息3s")
                else:
                    self.log.logger.info("请求任务失败：{}".format(data['message']))
            else:
                self.log.logger.info("请求任务失败，状态码：{}".format(response.status_code))
            time.sleep(3)
            timer = timer + 3
            # if timer > self.time_out:
            #     # 判断电量超过wait_power去招待点，小于low_power去充电
            #     batteryPercentage = self.get_power()
            #     self.log.logger.info("batteryPercentage="+str(batteryPercentage)+",self.current_point="+str(self.current_point)+",self.low_power"+str(self.low_power))
            #     if batteryPercentage > self.low_power and self.current_point != 2 and self.current_point != 3:
            #         self.current_point=2
            #         # self.go_wait()
            #     elif batteryPercentage <= self.low_power:
            #         if self.current_point != 3:
            #             self.current_point=3
            #             self.go_charge()
            #     elif batteryPercentage > self.wait_power and self.current_point == 3 :
            #         self.current_point=2
            #         # self.go_wait()

    def getUUID(self):
        mac_address = uuid.UUID(int=uuid.getnode()).hex[-12:].upper()
        mac_address = '-'.join([mac_address[i:i + 2] for i in range(0, 11, 2)])
        # 使用默认的接口enp1s0获取IP地址
        ip_address = self.get_ip_address()
        # ip_address = "192.168.11.148"
        self.log.logger.info("IP Address:", ip_address)
        # 使用默认的接口enp1s0获取MAC地址
        mac_address = self.get_mac_address()
        # mac_address = "48:b0:2d:d3:e5:79"
        self.log.logger.info("MAC Address:", mac_address)
        while True:
            param = {"robotIp": ip_address, "robotMac": mac_address, "robotType": "2"}
            response = requests.get(UUID_URL, params=param)
            if response.status_code == 200:
                data = json.loads(response.content)
                self.log.logger.info(data)
                robotCode = data['data']['robotCode']
                if robotCode != 'unbind' and robotCode != '':
                    self.log.logger.info("bind success!!!")
                    self.log.logger.info(str(data) + "=bind success")
                    self.car_id = data['data']['robotId']
                    self.robotCode = data['data']['robotCode']
                    break

            time.sleep(1)

    def get_ip_address(self, family=2, interface='eth0'):  # family=2 for IPv4, family=10 for IPv6
        for iface, sniff in psutil.net_if_addrs().items():
            if iface == interface:
                for item in sniff:
                    if item.family == family:
                        return item.address
        return None

    def get_mac_address(self, interface='eth0'):
        try:
            command = 'ip link show {} | grep "link/ether "'.format(interface)
            output = os.popen(command).read()
            lines = output.split('\n')

            for line in lines:
                if 'link/ether' in line:
                    mac_address = line.split()[1]
                    return mac_address
        except Exception as e:
            self.log.logger.info("Error:", e)
            return None

    def upload_data(self, content, filename):
        # pip install -U cos-python-sdk-v5
        response = client.put_object(
            Bucket='robot-1312773182',
            Body=content,
            Key=filename,
            EnableMD5=False
        )
        self.log.logger.info(response['ETag'])

    def crc16(self, data):
        crc = 0xFFFF  # 初始值为0xFFFF
        for byte in data:
            crc ^= byte  # 异或运算
            for _ in range(8):  # 移位操作
                if crc & 0x0001:  # 检查最低位
                    crc >>= 1  # 右移一位
                    crc ^= 0xA001  # 异或多项式
                else:
                    crc >>= 1  # 右移一位
        return crc & 0xFFFF  # 返回校验码的低16位

    def grip(self, action):
        param = {}
        param['speed'] = 500
        param['force'] = 500
        headers = {
            'Content-type': 'application/json',
            'Accept': 'application/json',
            'Token': self.token_v
        }
        open_url = ARM_HOST_HTTP + "/joint/setGripperRelease"
        close_url = ARM_HOST_HTTP + "/joint/setGripperPick"
        init_url = ARM_HOST_HTTP + "/arm/getArmSoftwareInfo"
        self.log.logger.info("夹爪控制param=" + str(param))
        if action == "open":
            response = requests.post(url=open_url, json=param, headers=headers)
            if response.status_code == 200:
                data = json.loads(response.content)
                self.log.logger.info("夹爪打开成功=" + str(data))
        elif action == "close":
            response = requests.post(url=close_url, json=param, headers=headers)
            if response.status_code == 200:
                data = json.loads(response.content)
                self.log.logger.info("夹爪关闭成功=" + str(data))
        elif action == "init":
            p = {}
            response = requests.post(url=init_url, json=p, headers=headers)
            if response.status_code == 200:
                data = json.loads(response.content)
                self.token_v = data['data']['token_v']
                print("夹爪初始化接口返回")
                print(data)
                self.grip("close")
                self.grip("open")

    def control_init(self):
        print("初始化机械臂和夹爪")
        # 初始化机械臂坐标系
        set_tool_frame_command = {"command": "set_change_tool_frame", "tool_name": "Gripper"}
        flag = self.set_tool_frame(set_tool_frame_command)
        print("设置工具坐标系为Gripper=" + str(flag))
        print("================初始化机械臂工具坐标系成功================")

        # 初始化机械臂位置
        self.send_arm_command(move_init_angle)
        print("================初始化机械臂位置成功================")

        # 初始化夹爪
        self.grip("init")
        self.suck.close_xipan()
        print("================初始化夹爪成功================")


if __name__ == '__main__':
    task = Task()
    task.getUUID()
    task.mqtt_init(debug=False)
    task.control_init()
    task.getTask()
